Repository navigation
fix(lidar): account for the sensor's mounting yaw in the bearing and the sensor odometry - #225
Open
qwist1233-cpu wants to merge 1 commit into
Open
qwist1233-cpu wants to merge 1 commit into
qwist1233-cpu wants to merge 1 commit into
Conversation
…the sensor odometry OmniDirectionalLidar measured the bearing of each contour point relative to the vehicle's heading only, while ScanPoint transformed the points back with the mounting yaw as well, so a sensor mounted at an angle drew its point cloud rotated by that angle. SensorParameters.calculate_sensor_odometry() built the sensor's pose with the vehicle's yaw alone, so the extrinsic calibrator compared against a pose without the mounting yaw. Subtract the mounting yaw from the bearing and add it to the sensor pose. Add a test that the noiseless points of a 30 deg mounted sensor lie on the obstacle's face and that the sensor odometry carries the mounting yaw. Closes ShisatoYano#211
This branch has not been deployed
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Closes #211
What
OmniDirectionalLidar.update()measured the bearing of each contour point against the vehicle's heading only (atan2(dy, dx) - state.get_yaw_rad()), whileScanPoint.get_transformed_data()rotates the points back with the mounting yaw as well, so a sensor mounted at an angle drew its whole point cloud rotated by that angle around the sensor.SensorParameters.calculate_sensor_odometry()had the same gap: it built the sensor's pose with the vehicle's yaw alone, soUkfExtParamsCalibratorcompared against a pose that lacked the mounting yaw.omni_directional_lidar.py: the bearing is now taken in the sensor frame,atan2(dy, dx) - (vehicle_yaw + INST_YAW_RAD).sensor_parameters.py:prev_sensor_tf/curr_sensor_tfare built withvehicle_yaw + INST_YAW_RAD.test/test_lidar_mounting_yaw.py: one 2 m x 2 m obstacle at (10, 0), sensor at the vehicle's origin, noise scales set to 0. With a 30° mounting yaw the transformed points must lie on the obstacle's near face (x = 9 m, |y| <= 1 m), and the sensor odometry yaw must equal vehicle yaw + mounting yaw.How it was verified
Before the fix, with a 30° mounting yaw the points landed at x = 7.3 to 8.3 m, y = 3.7 to 5.3 m (off the obstacle, matching the table in #211), and the sensor odometry yaw read 10° for a 10° vehicle + 30° mount; the new test fails on
mainfor exactly those two reasons. After the fix the 30° case gives the same face as the 0° case.The full suite (
pytest, 112 tests,test_mpc_path_tracking.pyskipped locally because casadi is not installed here) passes; every simulation keepsyaw_deg=0, so nothing else changes.